/*
 * RobotController.java
 *
 * Created on Dec 27, 2010 4:47:02 PM;
 */

package org.sunspotworld;

import CSC.NCSU.EDU.IRSensor;
import com.sun.spot.peripheral.IDriver;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.sensorboard.peripheral.Servo;
import com.sun.spot.sensorboard.EDemoBoard;
//import com.sun.spot.sensorboard.io.IScalarInput;
import com.sun.spot.sensorboard.io.IScalarInput;

import com.sun.spot.util.Utils;

import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 * The startApp method of this class is called by the VM to start the
 * application.
 *
 * The manifest specifies this class as MIDlet-1, which means it will
 * be selected for execution.
 */
public class RobotController extends MIDlet implements IDriver {


    private final int HALF_RANGE = 500;
    private Servo[] wheels = {new Servo(EDemoBoard.getInstance().getOutputPins()[0]), new Servo(EDemoBoard.getInstance().getOutputPins()[1])};
    private int [] stop = new int [2];
    private boolean servoActive = false;

    private IScalarInput right = EDemoBoard.getInstance().getScalarInputs()[EDemoBoard.A1];
    private IScalarInput left = EDemoBoard.getInstance().getScalarInputs()[EDemoBoard.A0];

    public IRSensor leftIR;
    public IRSensor rightIR;

    public int wheelLength = 11;
    public int totalThresholdChanges = 21;
    public int spokeCount = 0;



    protected void startApp() throws MIDletStateChangeException {

       Spot.getInstance().getDriverRegistry().add(this);
       stop[0] = 1450;  //the pulse width that holds servo 0 still (actual values will vary by servo)
       stop[1] = 1450;  //the pulse width that holds servo 1 still (actual values will vary by servo)
       for ( int i=0; i<2; i++) {
            wheels[i].setBounds(stop[i]-HALF_RANGE,stop[i]+HALF_RANGE);
       }

       leftIR = new IRSensor(left);
       rightIR = new IRSensor(right);

       forward(1);
       System.out.println("Left IR Count = " + leftIR.getCount());
       System.out.println("Right IR Count = " + rightIR.getCount());
       System.out.println("");

       notifyDestroyed();                      // cause the MIDlet to exit
    }

    /**
     * hold the servos still for a given amount of time
     * @param ms - number of milliseconds to brake
     */
    private void brake(long ms) {
        servoActive = true;
        for(int i=0; i<2; i++) {
            wheels[i].setPosition((float).5);  //position .5 corresponds to the stop value above
        }
        Utils.sleep(ms);
        servoActive = false;
    }

    /**
     * Brake for a quarter second and then turn off the servos
     */
    private void stop() {
        brake(250);
        for(int i=0; i<2; i++) {
            wheels[i].setValue(0);  //set servo pins low
        }
    }

    private void forward(int distance) {
        //Conversion method needed to turn distance into wheel counts.
        spokeCount = (distance/(wheelLength/totalThresholdChanges));
        leftIR.clearCount();
        rightIR.clearCount();
        servoActive = true;
        wheels[0].setPosition(0);
        wheels[1].setPosition(1);
        while( (leftIR.getCount() + rightIR.getCount())/2 < spokeCount ){
            //wait until you get to the right count.
        }
        stop();
        servoActive = false;
    }

    //
    //IDriver methods
    //


    /**
     * Implements getDriverName of IDriver interface
     * @return the name of this Driver
     */
    public String getDriverName() {
        return "RoverBot";
    }

    /**
     * Check if robot is ready for SPOT to deep sleep
     * @return whether the servos need to sustain a pulse
     */
    public boolean tearDown() {
        return !servoActive;
    }

    /**
     * what to do when SPOT comes out of deep sleep
     */
    public void setUp() {
        //do nothing
    }

    /**
     * what to do when SPOT shuts down
     */
    public void shutDown() {
        //do nothing
    }

    protected void pauseApp() {
        // This is not currently called by the Squawk VM
    }

    /**
     * Called if the MIDlet is terminated by the system.
     * I.e. if startApp throws any exception other than MIDletStateChangeException,
     * if the isolate running the MIDlet is killed with Isolate.exit(), or
     * if VM.stopVM() is called.
     *
     * It is not called if MIDlet.notifyDestroyed() was called.
     *
     * @param unconditional If true when this method is called, the MIDlet must
     *    cleanup and release all resources. If false the MIDlet may throw
     *    MIDletStateChangeException  to indicate it does not want to be destroyed
     *    at this time.
     */
    protected void destroyApp(boolean unconditional) throws MIDletStateChangeException {

    }
}
